/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/bits_math.h>
#include <mrpt/typemeta/TTypeName.h>
#include <mrpt/math/math_frwds.h>  // forward declarations
#include <mrpt/math/lightweight_geom_data_frwds.h>  // forward declarations
#include <mrpt/math/eigen_frwds.h>  // forward declarations
#include <mrpt/math/wrap2pi.h>
#include <mrpt/core/format.h>
#include <mrpt/core/exceptions.h>
#include <vector>
#include <stdexcept>
#include <type_traits>
#include <mrpt/serialization/serialization_frwds.h>

namespace mrpt
{
namespace math
{
/** Base type of all TPoseXX and TPointXX classes in mrpt::math.
 * Useful for type traits. No virtual methods at all. */
struct TPoseOrPoint
{
};

/** \addtogroup geometry_grp
 * @{ */

// Set of typedefs for lightweight geometric items.
/**
 * Lightweight 2D point. Allows coordinate access using [] operator.
 * \sa mrpt::poses::CPoint2D
 */
struct TPoint2D : public TPoseOrPoint
{
	enum
	{
		static_size = 2
	};
	/** X,Y coordinates */
	double x{.0}, y{.0};
	/** Constructor from TPose2D, discarding phi.
	 * \sa TPose2D
	 */
	explicit TPoint2D(const TPose2D& p);
	/**
	 * Constructor from TPoint3D, discarding z.
	 * \sa TPoint3D
	 */
	explicit TPoint2D(const TPoint3D& p);
	/**
	 * Constructor from TPose3D, discarding z and the angular coordinates.
	 * \sa TPose3D
	 */
	explicit TPoint2D(const TPose3D& p);

	/**
	 * Constructor from coordinates.
	 */
	constexpr TPoint2D(double xx, double yy) : x(xx), y(yy) {}
	/**
	 * Default fast constructor. Initializes to zeros.
	 */
	TPoint2D() = default;
	/** Coordinate access using operator[]. Order: x,y */
	double& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Coordinate access using operator[]. Order: x,y */
	constexpr const double& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			default:
				throw std::out_of_range("index out of range");
		}
	}

	/**
	 * Transformation into vector.
	 */
	void getAsVector(std::vector<double>& v) const
	{
		v.resize(2);
		v[0] = x;
		v[1] = y;
	}

	bool operator<(const TPoint2D& p) const;

	TPoint2D& operator+=(const TPoint2D& p)
	{
		x += p.x;
		y += p.y;
		return *this;
	}

	TPoint2D& operator-=(const TPoint2D& p)
	{
		x -= p.x;
		y -= p.y;
		return *this;
	}

	TPoint2D& operator*=(double d)
	{
		x *= d;
		y *= d;
		return *this;
	}

	TPoint2D& operator/=(double d)
	{
		x /= d;
		y /= d;
		return *this;
	}

	constexpr TPoint2D operator+(const TPoint2D& p) const
	{
		return {x + p.x, y + p.y};
	}

	constexpr TPoint2D operator-(const TPoint2D& p) const
	{
		return {x - p.x, y - p.y};
	}

	constexpr TPoint2D operator*(double d) const { return {d * x, d * y}; }
	constexpr TPoint2D operator/(double d) const { return {x / d, y / d}; }
	/** Returns a human-readable textual representation of the object (eg:
	 * "[0.02 1.04]" )
	 * \sa fromString
	 */
	void asString(std::string& s) const { s = mrpt::format("[%f %f]", x, y); }
	std::string asString() const
	{
		std::string s;
		asString(s);
		return s;
	}

	/** Set the current object value from a string generated by 'asString' (eg:
	 * "[0.02 1.04]" )
	 * \sa asString
	 * \exception std::exception On invalid format
	 */
	void fromString(const std::string& s);
	static size_t size() { return 2; }
	/** Point norm. */
	double norm() const { return sqrt(square(x) + square(y)); }
};

/**
 * Lightweight 2D pose. Allows coordinate access using [] operator.
 * \sa mrpt::poses::CPose2D
 */
struct TPose2D : public TPoseOrPoint
{
	enum
	{
		static_size = 3
	};
	/** X,Y coordinates */
	double x{.0}, y{.0};
	/** Orientation (rads) */
	double phi{.0};

	/** Returns the identity transformation */
	static constexpr TPose2D Identity() { return TPose2D(); }

	/** Implicit constructor from TPoint2D. Zeroes the phi coordinate.
	 * \sa TPoint2D
	 */
	TPose2D(const TPoint2D& p);
	/**
	 * Constructor from TPoint3D, losing information. Zeroes the phi
	 * coordinate.
	 * \sa TPoint3D
	 */
	explicit TPose2D(const TPoint3D& p);
	/**
	 * Constructor from TPose3D, losing information. The phi corresponds to the
	 * original pose's yaw.
	 * \sa TPose3D
	 */
	explicit TPose2D(const TPose3D& p);
	/**
	 * Constructor from coordinates.
	 */
	constexpr TPose2D(double xx, double yy, double Phi) : x(xx), y(yy), phi(Phi)
	{
	}
	/**
	 * Default fast constructor. Initializes to zeros.
	 */
	constexpr TPose2D() = default;
	/** Coordinate access using operator[]. Order: x,y,phi */
	double& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return phi;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Coordinate access using operator[]. Order: x,y,phi */
	constexpr const double& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return phi;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/**
	 * Transformation into vector.
	 */
	void getAsVector(std::vector<double>& v) const
	{
		v.resize(3);
		v[0] = x;
		v[1] = y;
		v[2] = phi;
	}
	/** Returns a human-readable textual representation of the object (eg: "[x y
	 * yaw]", yaw in degrees)
	 * \sa fromString
	 */
	void asString(std::string& s) const;
	std::string asString() const
	{
		std::string s;
		asString(s);
		return s;
	}

	/** Operator "oplus" pose composition: "ret=this \oplus b"  \sa CPose2D */
	mrpt::math::TPose2D operator+(const mrpt::math::TPose2D& b) const;
	/** Operator "ominus" pose composition: "ret=this \ominus b"  \sa CPose2D */
	mrpt::math::TPose2D operator-(const mrpt::math::TPose2D& b) const;

	mrpt::math::TPoint2D composePoint(const TPoint2D l) const
	{
		const double ccos = ::cos(phi), csin = ::sin(phi);
		return {x + l.x * ccos - l.y * csin, y + l.x * csin + l.y * ccos};
	}

	mrpt::math::TPoint2D operator+(const mrpt::math::TPoint2D& b) const
	{
		return this->composePoint(b);
	}

	mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
	{
		const double Ax = g.x - x, Ay = g.y - y, ccos = ::cos(phi),
					 csin = ::sin(phi);
		return {Ax * ccos + Ay * csin, -Ax * csin + Ay * ccos};
	}

	/** Returns the norm of the (x,y) vector (phi is not used) */
	double norm() const { return mrpt::hypot_fast(x, y); }
	/** Forces "phi" to be in the range [-pi,pi] */
	void normalizePhi() { phi = mrpt::math::wrapToPi(phi); }
	/** Set the current object value from a string generated by 'asString' (eg:
	 * "[0.02 1.04 -45.0]" )
	 * \sa asString
	 * \exception std::exception On invalid format
	 */
	void fromString(const std::string& s);

	constexpr static size_t size() { return 3; }
};

/** Lightweight 3D point (float version).
 * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3D
 */
struct TPoint3Df : public TPoseOrPoint
{
	enum
	{
		static_size = 3
	};
	float x{.0f}, y{.0f}, z{.0f};

	TPoint3Df() = default;
	constexpr TPoint3Df(const float xx, const float yy, const float zz)
		: x(xx), y(yy), z(zz)
	{
	}
	TPoint3Df& operator+=(const TPoint3Df& p)
	{
		x += p.x;
		y += p.y;
		z += p.z;
		return *this;
	}
	TPoint3Df operator*(const float s)
	{
		return TPoint3Df(x * s, y * s, z * s);
	}
	/** Coordinate access using operator[]. Order: x,y,z */
	float& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			default:
				throw std::out_of_range("index out of range");
		}
	}

	/** Coordinate access using operator[]. Order: x,y,z */
	constexpr const float& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			default:
				throw std::out_of_range("index out of range");
		}
	}
};

/**
 * Lightweight 3D point. Allows coordinate access using [] operator.
 * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
 */
struct TPoint3D : public TPoseOrPoint
{
	enum
	{
		static_size = 3
	};
	/** X,Y,Z coordinates */
	double x{.0}, y{.0}, z{.0};

	/** Constructor from coordinates.  */
	constexpr TPoint3D(double xx, double yy, double zz) : x(xx), y(yy), z(zz) {}
	/** Default fast constructor. Initializes to zeros. */
	TPoint3D() = default;
	/** Explicit constructor from coordinates.  */
	explicit TPoint3D(const TPoint3Df& p) : x(p.x), y(p.y), z(p.z) {}
	/** Implicit constructor from TPoint2D. Zeroes the z.
	 * \sa TPoint2D
	 */
	TPoint3D(const TPoint2D& p);
	/**
	 * Constructor from TPose2D, losing information. Zeroes the z.
	 * \sa TPose2D
	 */
	explicit TPoint3D(const TPose2D& p);
	/**
	 * Constructor from TPose3D, losing information.
	 * \sa TPose3D
	 */
	explicit TPoint3D(const TPose3D& p);
	/** Coordinate access using operator[]. Order: x,y,z */
	double& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Coordinate access using operator[]. Order: x,y,z */
	constexpr const double& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/**
	 * Point-to-point distance.
	 */
	double distanceTo(const TPoint3D& p) const
	{
		return sqrt(square(p.x - x) + square(p.y - y) + square(p.z - z));
	}
	/**
	 * Point-to-point distance, squared.
	 */
	double sqrDistanceTo(const TPoint3D& p) const
	{
		return square(p.x - x) + square(p.y - y) + square(p.z - z);
	}
	/**
	 * Point norm.
	 */
	double norm() const { return sqrt(square(x) + square(y) + square(z)); }
	/**
	 * Point scale.
	 */
	TPoint3D& operator*=(const double f)
	{
		x *= f;
		y *= f;
		z *= f;
		return *this;
	}
	/**
	 * Transformation into vector.
	 */
	template <class VECTORLIKE>
	void getAsVector(VECTORLIKE& v) const
	{
		v.resize(3);
		v[0] = x;
		v[1] = y;
		v[2] = z;
	}
	/**
	 * Translation.
	 */
	TPoint3D& operator+=(const TPoint3D& p)
	{
		x += p.x;
		y += p.y;
		z += p.z;
		return *this;
	}
	/**
	 * Difference between points.
	 */
	TPoint3D& operator-=(const TPoint3D& p)
	{
		x -= p.x;
		y -= p.y;
		z -= p.z;
		return *this;
	}
	/**
	 * Points addition.
	 */
	constexpr TPoint3D operator+(const TPoint3D& p) const
	{
		return {x + p.x, y + p.y, z + p.z};
	}
	/**
	 * Points substraction.
	 */
	constexpr TPoint3D operator-(const TPoint3D& p) const
	{
		return {x - p.x, y - p.y, z - p.z};
	}

	constexpr TPoint3D operator*(double d) const
	{
		return {x * d, y * d, z * d};
	}

	constexpr TPoint3D operator/(double d) const
	{
		return {x / d, y / d, z / d};
	}

	bool operator<(const TPoint3D& p) const;

	/** Returns a human-readable textual representation of the object (eg:
	 * "[0.02 1.04 -0.8]" )
	 * \sa fromString
	 */
	void asString(std::string& s) const
	{
		s = mrpt::format("[%f %f %f]", x, y, z);
	}
	std::string asString() const
	{
		std::string s;
		asString(s);
		return s;
	}

	/** Set the current object value from a string generated by 'asString' (eg:
	 * "[0.02 1.04 -0.8]" )
	 * \sa asString
	 * \exception std::exception On invalid format
	 */
	void fromString(const std::string& s);
	static constexpr size_t size() { return 3; }
};

/** XYZ point (double) + Intensity(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZIu8
{
	mrpt::math::TPoint3D pt;
	uint8_t intensity{0};
	TPointXYZIu8() : pt() {}
	constexpr TPointXYZIu8(double x, double y, double z, uint8_t intensity_val)
		: pt(x, y, z), intensity(intensity_val)
	{
	}
};
/** XYZ point (double) + RGB(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZRGBu8
{
	mrpt::math::TPoint3D pt;
	uint8_t R{0}, G{0}, B{0};
	TPointXYZRGBu8() = default;
	constexpr TPointXYZRGBu8(
		double x, double y, double z, uint8_t R_val, uint8_t G_val,
		uint8_t B_val)
		: pt(x, y, z), R(R_val), G(G_val), B(B_val)
	{
	}
};
/** XYZ point (float) + Intensity(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZfIu8
{
	mrpt::math::TPoint3Df pt;
	uint8_t intensity{0};
	TPointXYZfIu8() = default;
	constexpr TPointXYZfIu8(float x, float y, float z, uint8_t intensity_val)
		: pt(x, y, z), intensity(intensity_val)
	{
	}
};
/** XYZ point (float) + RGB(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZfRGBu8
{
	mrpt::math::TPoint3Df pt;
	uint8_t R{0}, G{0}, B{0};
	TPointXYZfRGBu8() : pt() {}
	constexpr TPointXYZfRGBu8(
		float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
		: pt(x, y, z), R(R_val), G(G_val), B(B_val)
	{
	}
};

/**
 * Lightweight 3D pose (three spatial coordinates, plus three angular
 * coordinates). Allows coordinate access using [] operator.
 * \sa mrpt::poses::CPose3D
 */
struct TPose3D : public TPoseOrPoint
{
	enum
	{
		static_size = 6
	};
	/** X,Y,Z, coords */
	double x{.0}, y{.0}, z{.0};
	/** Yaw coordinate (rotation angle over Z axis). */
	double yaw{.0};
	/** Pitch coordinate (rotation angle over Y axis). */
	double pitch{.0};
	/** Roll coordinate (rotation angle over X coordinate). */
	double roll{.0};

	/** Returns the identity transformation, T=eye(4) */
	static constexpr TPose3D Identity() { return TPose3D(); }

	/** Implicit constructor from TPoint2D. Zeroes all the unprovided
	 * information.
	 * \sa TPoint2D
	 */
	TPose3D(const TPoint2D& p);
	/**
	 * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi,
	 * zeroing all the unprovided information.
	 * \sa TPose2D
	 */
	TPose3D(const TPose2D& p);
	/**
	 * Implicit constructor from TPoint3D. Zeroes angular information.
	 * \sa TPoint3D
	 */
	TPose3D(const TPoint3D& p);
	/**
	 * Constructor from coordinates.
	 */
	constexpr TPose3D(
		double _x, double _y, double _z, double _yaw, double _pitch,
		double _roll)
		: x(_x), y(_y), z(_z), yaw(_yaw), pitch(_pitch), roll(_roll)
	{
	}
	/**
	 * Default fast constructor. Initializes to zeros.
	 */
	constexpr TPose3D() = default;
	/** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
	double& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			case 3:
				return yaw;
			case 4:
				return pitch;
			case 5:
				return roll;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
	constexpr const double& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			case 3:
				return yaw;
			case 4:
				return pitch;
			case 5:
				return roll;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/**
	 * Pose's spatial coordinates norm.
	 */
	double norm() const { return std::sqrt(square(x) + square(y) + square(z)); }
	/**
	 * Gets the pose as a vector of doubles.
	 */
	void getAsVector(std::vector<double>& v) const
	{
		v.resize(6);
		v[0] = x;
		v[1] = y;
		v[2] = z;
		v[3] = yaw;
		v[4] = pitch;
		v[5] = roll;
	}
	/** Returns a human-readable textual representation of the object (eg: "[x y
	 * z yaw pitch roll]", angles in degrees.)
	 * \sa fromString
	 */
	void asString(std::string& s) const;
	std::string asString() const
	{
		std::string s;
		asString(s);
		return s;
	}

	/** Returns the quaternion associated to the rotation of this object (NOTE:
	 * XYZ translation is ignored)
	 * \f[ \mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2)
	 * \cos (\psi /2) +  \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin
	 * (\phi /2) \cos (\theta /2) \cos (\psi /2) -  \cos (\phi /2) \sin (\theta
	 * /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) +
	 * \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos
	 * (\theta /2) \sin (\psi /2) -  \sin (\phi /2) \sin (\theta /2) \cos (\psi
	 * /2) \\ \end{array}\right) \f]
	 * With : \f$ \phi = roll \f$,  \f$ \theta = pitch \f$ and \f$ \psi = yaw
	 * \f$.
	 * \param out_dq_dr  If provided, the 4x3 Jacobian of the transformation
	 * will be computed and stored here. It's the Jacobian of the transformation
	 * from (yaw pitch roll) to (qr qx qy qz).
	 */
	void getAsQuaternion(
		mrpt::math::CQuaternion<double>& q,
		mrpt::math::CMatrixFixedNumeric<double, 4, 3>* out_dq_dr =
			nullptr) const;

	void composePoint(const TPoint3D l, TPoint3D& g) const;
	void inverseComposePoint(const TPoint3D g, TPoint3D& l) const;
	void composePose(const TPose3D other, TPose3D& result) const;
	void getRotationMatrix(mrpt::math::CMatrixDouble33& R) const;
	void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const;
	void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const;
	void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44& HG);
	static void SO3_to_yaw_pitch_roll(
		const mrpt::math::CMatrixDouble33& R, double& yaw, double& pitch,
		double& roll);
	/** Set the current object value from a string generated by 'asString' (eg:
	 * "[0.02 1.04 -0.8]" )
	 * \sa asString
	 * \exception std::exception On invalid format
	 */
	void fromString(const std::string& s);
	static size_t size() { return 6; }
};

/** Unary $\ominus\$ operator: computes inverse SE(3) element */
TPose3D operator-(const TPose3D& p);
/** Binary $\ominus\$ operator: \$b \ominus a\$ computes the relative SE(3) pose
 * of `b` "as seen from" `a` */
TPose3D operator-(const TPose3D& b, const TPose3D& a);

/** Lightweight 3D pose (three spatial coordinates, plus a quaternion ). Allows
 * coordinate access using [] operator.
 * \sa mrpt::poses::CPose3DQuat
 */
struct TPose3DQuat : public TPoseOrPoint
{
	enum
	{
		static_size = 7
	};
	/** Translation in x,y,z */
	double x{.0}, y{.0}, z{.0};
	/** Unit quaternion part, qr,qx,qy,qz */
	double qr{1.}, qx{.0}, qy{.0}, qz{.0};

	/** Constructor from coordinates. */
	constexpr TPose3DQuat(
		double _x, double _y, double _z, double _qr, double _qx, double _qy,
		double _qz)
		: x(_x), y(_y), z(_z), qr(_qr), qx(_qx), qy(_qy), qz(_qz)
	{
	}
	/** Default fast constructor. Initializes to identity transformation. */
	TPose3DQuat() = default;
	/** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
	double& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			case 3:
				return qr;
			case 4:
				return qx;
			case 5:
				return qy;
			case 6:
				return qz;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
	constexpr const double& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			case 3:
				return qr;
			case 4:
				return qx;
			case 5:
				return qy;
			case 6:
				return qz;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Pose's spatial coordinates norm. */
	double norm() const { return sqrt(square(x) + square(y) + square(z)); }
	/** Gets the pose as a vector of doubles. */
	void getAsVector(std::vector<double>& v) const
	{
		v.resize(7);
		for (size_t i = 0; i < 7; i++) v[i] = (*this)[i];
	}
	/** Returns a human-readable textual representation of the object as "[x y z
	 * qr qx qy qz]"
	 * \sa fromString
	 */
	void asString(std::string& s) const
	{
		s = mrpt::format("[%f %f %f %f %f %f %f]", x, y, z, qr, qx, qy, qz);
	}
	std::string asString() const
	{
		std::string s;
		asString(s);
		return s;
	}

	/** Set the current object value from a string generated by 'asString' (eg:
	 * "[0.02 1.04 -0.8 1.0 0.0 0.0 0.0]" )
	 * \sa asString
	 * \exception std::exception On invalid format
	 */
	void fromString(const std::string& s);
	static size_t size() { return 7; }
};

// Text streaming functions:
std::ostream& operator<<(std::ostream& o, const TPoint2D& p);
std::ostream& operator<<(std::ostream& o, const TPoint3D& p);
std::ostream& operator<<(std::ostream& o, const TPose2D& p);
std::ostream& operator<<(std::ostream& o, const TPose3D& p);
std::ostream& operator<<(std::ostream& o, const TPose3DQuat& p);

/**
 * Unary minus operator for 3D points.
 */
constexpr TPoint3D operator-(const TPoint3D& p1)
{
	return {-p1.x, -p1.y, -p1.z};
}
/**
 * Exact comparison between 2D points.
 */
constexpr bool operator==(const TPoint2D& p1, const TPoint2D& p2)
{
	return (p1.x == p2.x) && (p1.y == p2.y);  //-V550
}
/**
 * Exact comparison between 2D points.
 */
constexpr bool operator!=(const TPoint2D& p1, const TPoint2D& p2)
{
	return (p1.x != p2.x) || (p1.y != p2.y);  //-V550
}
/**
 * Exact comparison between 3D points.
 */
constexpr bool operator==(const TPoint3D& p1, const TPoint3D& p2)
{
	return (p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z);  //-V550
}
/**
 * Exact comparison between 3D points.
 */
constexpr bool operator!=(const TPoint3D& p1, const TPoint3D& p2)
{
	return (p1.x != p2.x) || (p1.y != p2.y) || (p1.z != p2.z);  //-V550
}
/**
 * Exact comparison between 2D poses, taking possible cycles into account.
 */
inline bool operator==(const TPose2D& p1, const TPose2D& p2)
{
	return (p1.x == p2.x) && (p1.y == p2.y) &&
		   (mrpt::math::wrapTo2Pi(p1.phi) ==
			mrpt::math::wrapTo2Pi(p2.phi));  //-V550
}
/**
 * Exact comparison between 2D poses, taking possible cycles into account.
 */
inline bool operator!=(const TPose2D& p1, const TPose2D& p2)
{
	return (p1.x != p2.x) || (p1.y != p2.y) ||
		   (mrpt::math::wrapTo2Pi(p1.phi) !=
			mrpt::math::wrapTo2Pi(p2.phi));  //-V550
}
/**
 * Exact comparison between 3D poses, taking possible cycles into account.
 */
inline bool operator==(const TPose3D& p1, const TPose3D& p2)
{
	return (p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z) &&
		   (mrpt::math::wrapTo2Pi(p1.yaw) == mrpt::math::wrapTo2Pi(p2.yaw)) &&
		   (mrpt::math::wrapTo2Pi(p1.pitch) ==
			mrpt::math::wrapTo2Pi(p2.pitch)) &&
		   (mrpt::math::wrapTo2Pi(p1.roll) ==
			mrpt::math::wrapTo2Pi(p2.roll));  //-V550
}
/**
 * Exact comparison between 3D poses, taking possible cycles into account.
 */
inline bool operator!=(const TPose3D& p1, const TPose3D& p2)
{
	return (p1.x != p2.x) || (p1.y != p2.y) || (p1.z != p2.z) ||
		   (mrpt::math::wrapTo2Pi(p1.yaw) != mrpt::math::wrapTo2Pi(p2.yaw)) ||
		   (mrpt::math::wrapTo2Pi(p1.pitch) !=
			mrpt::math::wrapTo2Pi(p2.pitch)) ||
		   (mrpt::math::wrapTo2Pi(p1.roll) !=
			mrpt::math::wrapTo2Pi(p2.roll));  //-V550
}
// Forward declarations
struct TSegment3D;
struct TLine3D;
class TPolygon3D;
struct TObject3D;

// Pragma defined to ensure no structure packing
/**
 * 2D segment, consisting of two points.
 * \sa TSegment3D,TLine2D,TPolygon2D,TPoint2D
 */
struct TSegment2D
{
   public:
	/**
	 * Origin point.
	 */
	TPoint2D point1;
	/**
	 * Destiny point.
	 */
	TPoint2D point2;
	/**
	 * Segment length.
	 */
	double length() const;
	/**
	 * Distance to point.
	 */
	double distance(const TPoint2D& point) const;
	/**
	 * Distance with sign to point (sign indicates which side the point is).
	 */
	double signedDistance(const TPoint2D& point) const;
	/**
	 * Check whether a point is inside a segment.
	 */
	bool contains(const TPoint2D& point) const;
	/** Access to points using operator[0-1] */
	TPoint2D& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return point1;
			case 1:
				return point2;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Access to points using operator[0-1] */
	constexpr const TPoint2D& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return point1;
			case 1:
				return point2;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/**
	 * Project into 3D space, setting the z to 0.
	 */
	void generate3DObject(TSegment3D& s) const;
	/**
	 * Segment's central point.
	 */
	void getCenter(TPoint2D& p) const
	{
		p.x = (point1.x + point2.x) / 2;
		p.y = (point1.y + point2.y) / 2;
	}
	/**
	 * Constructor from both points.
	 */
	TSegment2D(const TPoint2D& p1, const TPoint2D& p2) : point1(p1), point2(p2)
	{
	}
	/**
	 * Fast default constructor. Initializes to garbage.
	 */
	TSegment2D() = default;
	/**
	 * Explicit constructor from 3D object, discarding the z.
	 */
	explicit TSegment2D(const TSegment3D& s);

	bool operator<(const TSegment2D& s) const;
};
/**
 * 3D segment, consisting of two points.
 * \sa TSegment2D,TLine3D,TPlane,TPolygon3D,TPoint3D
 */
struct TSegment3D
{
   public:
	/**
	 * Origin point.
	 */
	TPoint3D point1;
	/**
	 * Destiny point.
	 */
	TPoint3D point2;
	/**
	 * Segment length.
	 */
	double length() const;
	/**
	 * Distance to point.
	 */
	double distance(const TPoint3D& point) const;
	/**
	 * Distance to another segment.
	 */
	double distance(const TSegment3D& segment) const;
	/**
	 * Check whether a point is inside the segment.
	 */
	bool contains(const TPoint3D& point) const;
	/** Access to points using operator[0-1] */
	TPoint3D& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return point1;
			case 1:
				return point2;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Access to points using operator[0-1] */
	const TPoint3D& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return point1;
			case 1:
				return point2;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/**
	 * Projection into 2D space, discarding the z.
	 */
	void generate2DObject(TSegment2D& s) const { s = TSegment2D(*this); }
	/**
	 * Segment's central point.
	 */
	void getCenter(TPoint3D& p) const
	{
		p.x = (point1.x + point2.x) / 2;
		p.y = (point1.y + point2.y) / 2;
		p.z = (point1.z + point2.z) / 2;
	}
	/**
	 * Constructor from both points.
	 */
	TSegment3D(const TPoint3D& p1, const TPoint3D& p2) : point1(p1), point2(p2)
	{
	}
	/**
	 * Fast default constructor. Initializes to garbage.
	 */
	TSegment3D() = default;
	/**
	 * Constructor from 2D object. Sets the z to zero.
	 */
	explicit TSegment3D(const TSegment2D& s)
		: point1(s.point1), point2(s.point2)
	{
	}

	bool operator<(const TSegment3D& s) const;
};

inline bool operator==(const TSegment2D& s1, const TSegment2D& s2)
{
	return (s1.point1 == s2.point1) && (s1.point2 == s2.point2);
}

inline bool operator!=(const TSegment2D& s1, const TSegment2D& s2)
{
	return (s1.point1 != s2.point1) || (s1.point2 != s2.point2);
}

inline bool operator==(const TSegment3D& s1, const TSegment3D& s2)
{
	return (s1.point1 == s2.point1) && (s1.point2 == s2.point2);
}

inline bool operator!=(const TSegment3D& s1, const TSegment3D& s2)
{
	return (s1.point1 != s2.point1) || (s1.point2 != s2.point2);
}

/**
 * 2D line without bounds, represented by its equation \f$Ax+By+C=0\f$.
 * \sa TLine3D,TSegment2D,TPolygon2D,TPoint2D
 */
struct TLine2D
{
   public:
	/**
	 * Line coefficients, stored as an array: \f$\left[A,B,C\right]\f$.
	 */
	double coefs[3] = {0, 0, 0};
	/**
	 * Evaluate point in the line's equation.
	 */
	double evaluatePoint(const TPoint2D& point) const;
	/**
	 * Check whether a point is inside the line.
	 */
	bool contains(const TPoint2D& point) const;
	/**
	 * Distance from a given point.
	 */
	double distance(const TPoint2D& point) const;
	/**
	 * Distance with sign from a given point (sign indicates side).
	 */
	double signedDistance(const TPoint2D& point) const;
	/**
	 * Get line's normal vector.
	 */
	void getNormalVector(double (&vector)[2]) const;
	/**
	 * Unitarize line's normal vector.
	 */
	void unitarize();
	/**
	 * Get line's normal vector after unitarizing line.
	 */
	void getUnitaryNormalVector(double (&vector)[2])
	{
		unitarize();
		getNormalVector(vector);
	}
	/**
	 * Get line's director vector.
	 */
	void getDirectorVector(double (&vector)[2]) const;
	/**
	 * Unitarize line and then get director vector.
	 */
	void getUnitaryDirectorVector(double (&vector)[2])
	{
		unitarize();
		getDirectorVector(vector);
	}
	/**
	 * Project into 3D space, setting the z to 0.
	 */
	void generate3DObject(TLine3D& l) const;
	/**
	 * Constructor from two points, through which the line will pass.
	 * \throw logic_error if both points are the same
	 */
	TLine2D(const TPoint2D& p1, const TPoint2D& p2);
	/**
	 * Constructor from a segment.
	 */
	explicit TLine2D(const TSegment2D& s);
	/**
	 * Fast default constructor. Initializes to garbage.
	 */
	TLine2D() = default;
	/**
	 * Constructor from line's coefficients.
	 */
	constexpr TLine2D(double A, double B, double C) : coefs{A, B, C} {}
	/**
	 * Construction from 3D object, discarding the Z.
	 * \throw std::logic_error if the line is normal to the XY plane.
	 */
	explicit TLine2D(const TLine3D& l);
	void getAsPose2D(TPose2D& outPose) const;
	void getAsPose2DForcingOrigin(
		const TPoint2D& origin, TPose2D& outPose) const;
};

/**
 * 3D line, represented by a base point and a director vector.
 * \sa TLine2D,TSegment3D,TPlane,TPolygon3D,TPoint3D
 */
struct TLine3D
{
   public:
	/**
	 * Base point.
	 */
	TPoint3D pBase;
	/**
	 * Director vector.
	 */
	double director[3] = {.0, .0, .0};
	/**
	 * Check whether a point is inside the line.
	 */
	bool contains(const TPoint3D& point) const;
	/**
	 * Distance between the line and a point.
	 */
	double distance(const TPoint3D& point) const;
	/**
	 * Unitarize director vector.
	 */
	void unitarize();
	/**
	 * Get director vector.
	 */
	void getDirectorVector(double (&vector)[3]) const
	{
		for (size_t i = 0; i < 3; i++) vector[i] = director[i];
	}
	/**
	 * Unitarize and then get director vector.
	 */
	void getUnitaryDirectorVector(double (&vector)[3])
	{
		unitarize();
		getDirectorVector(vector);
	}
	/**
	 * Project into 2D space, discarding the Z coordinate.
	 * \throw std::logic_error if the line's director vector is orthogonal to
	 * the XY plane.
	 */
	void generate2DObject(TLine2D& l) const { l = TLine2D(*this); }
	/**
	 * Constructor from two points, through which the line will pass.
	 * \throw std::logic_error if both points are the same.
	 */
	TLine3D(const TPoint3D& p1, const TPoint3D& p2);
	/**
	 * Constructor from 3D segment.
	 */
	explicit TLine3D(const TSegment3D& s);
	/**
	 * Fast default constructor. Initializes to garbage.
	 */
	TLine3D() = default;
	/** Constructor from 2D object. Zeroes the z. */
	explicit TLine3D(const TLine2D& l);
};

/**
 * 3D Plane, represented by its equation \f$Ax+By+Cz+D=0\f$
 * \sa TSegment3D,TLine3D,TPolygon3D,TPoint3D
 */
struct TPlane
{
   public:
	/**
	 * Plane coefficients, stored as an array: \f$\left[A,B,C,D\right]\f$
	 */
	double coefs[4] = {.0, .0, .0, .0};
	/**
	 * Evaluate a point in the plane's equation.
	 */
	double evaluatePoint(const TPoint3D& point) const;
	/**
	 * Check whether a point is contained into the plane.
	 */
	bool contains(const TPoint3D& point) const;
	/**
	 * Check whether a segment is fully contained into the plane.
	 */
	bool contains(const TSegment3D& segment) const
	{
		return contains(segment.point1) && contains(segment.point2);
	}
	/**
	 * Check whether a line is fully contained into the plane.
	 */
	bool contains(const TLine3D& line) const;
	/**
	 * Distance to 3D point.
	 */
	double distance(const TPoint3D& point) const;
	/**
	 * Distance to 3D line. Will be zero if the line is not parallel to the
	 * plane.
	 */
	double distance(const TLine3D& line) const;
	/**
	 * Get plane's normal vector.
	 */
	void getNormalVector(double (&vec)[3]) const;
	/**
	 * Unitarize normal vector.
	 */
	void unitarize();
	void getAsPose3D(mrpt::math::TPose3D& outPose);
	/**
	 * Unitarize, then get normal vector.
	 */
	void getUnitaryNormalVector(double (&vec)[3])
	{
		unitarize();
		getNormalVector(vec);
	}
	/**
	 * Gets a plane which contains these three points.
	 * \throw std::logic_error if the points are linearly dependants.
	 */
	TPlane(const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3);
	/**
	 * Gets a plane which contains this point and this line.
	 * \throw std::logic_error if the point is inside the line.
	 */
	TPlane(const TPoint3D& p1, const TLine3D& r2);
	/**
	 * Gets a plane which contains the two lines.
	 * \throw std::logic_error if the lines do not cross.
	 */
	TPlane(const TLine3D& r1, const TLine3D& r2);
	/**
	 * Fast default constructor. Initializes to garbage.
	 */
	TPlane() = default;
	/**
	 * Constructor from plane coefficients.
	 */
	constexpr TPlane(double A, double B, double C, double D) : coefs{A, B, C, D}
	{
	}
	/**
	 * Constructor from an array of coefficients.
	 */
	TPlane(const double (&vec)[4])
	{
		for (size_t i = 0; i < 4; i++) coefs[i] = vec[i];
	}
	void getAsPose3DForcingOrigin(const TPoint3D& newOrigin, TPose3D& pose);
};

using TPlane3D = TPlane;

/**
 * 2D polygon, inheriting from std::vector<TPoint2D>.
 * \sa TPolygon3D,TSegment2D,TLine2D,TPoint2D, CPolygon
 */
class TPolygon2D : public std::vector<TPoint2D>
{
   public:
	/** Distance to a point (always >=0) */
	double distance(const TPoint2D& point) const;
	/** Check whether a point is inside (or within geometryEpsilon of a polygon
	 * edge). This works for concave or convex polygons. */
	bool contains(const TPoint2D& point) const;
	/** Gets as set of segments, instead of points. */
	void getAsSegmentList(std::vector<TSegment2D>& v) const;
	/** Projects into 3D space, zeroing the z. */
	void generate3DObject(TPolygon3D& p) const;
	/** Polygon's central point. */
	void getCenter(TPoint2D& p) const;
	/** Checks whether is convex. */
	bool isConvex() const;
	/** Erase repeated vertices.  \sa removeRedundantVertices */
	void removeRepeatedVertices();
	/** Erase every redundant vertex from the polygon, saving space. \sa
	 * removeRepeatedVertices */
	void removeRedundantVertices();
	/** Gets plot data, ready to use on a 2D plot. \sa
	 * mrpt::gui::CDisplayWindowPlots */
	void getPlotData(std::vector<double>& x, std::vector<double>& y) const;
	/** Get polygon bounding box. \exception On empty polygon */
	void getBoundingBox(TPoint2D& min_coords, TPoint2D& max_coords) const;
	/** Default constructor  */
	TPolygon2D() : std::vector<TPoint2D>() {}
	/** Constructor for a given number of vertices, intializing them as garbage.
	 */
	explicit TPolygon2D(size_t N) : std::vector<TPoint2D>(N) {}
	/** Implicit constructor from a vector of 2D points */
	TPolygon2D(const std::vector<TPoint2D>& v) : std::vector<TPoint2D>(v) {}
	/** Constructor from a 3D object. */
	explicit TPolygon2D(const TPolygon3D& p);
	/** Static method to create a regular polygon, given its size and radius.
	 * \throw std::logic_error if radius is near zero or the number of edges is
	 * less than three.
	 */
	static void createRegularPolygon(
		size_t numEdges, double radius, TPolygon2D& poly);
	/** Static method to create a regular polygon from its size and radius. The
	 * center will correspond to the given pose.
	 * \throw std::logic_error if radius is near zero or the number of edges is
	 * less than three.
	 */
	static void createRegularPolygon(
		size_t numEdges, double radius, TPolygon2D& poly,
		const mrpt::math::TPose2D& pose);
};

/**
 * 3D polygon, inheriting from std::vector<TPoint3D>
 * \sa TPolygon2D,TSegment3D,TLine3D,TPlane,TPoint3D
 */
class TPolygon3D : public std::vector<TPoint3D>
{
   public:
	/** Distance to point (always >=0) */
	double distance(const TPoint3D& point) const;
	/** Check whether a point is inside (or within geometryEpsilon of a polygon
	 * edge). This works for concave or convex polygons. */
	bool contains(const TPoint3D& point) const;
	/** Gets as set of segments, instead of set of points. */
	void getAsSegmentList(std::vector<TSegment3D>& v) const;
	/** Gets a plane which contains the polygon. Returns false if the polygon is
	 * skew and cannot be fit inside a plane. */
	bool getPlane(TPlane& p) const;
	/** Gets the best fitting plane, disregarding whether the polygon actually
	 * fits inside or not. \sa getBestFittingPlane */
	void getBestFittingPlane(TPlane& p) const;
	/** Projects into a 2D space, discarding the z. \sa getPlane,isSkew */
	void generate2DObject(TPolygon2D& p) const { p = TPolygon2D(*this); }
	/** Get polygon's central point. */
	void getCenter(TPoint3D& p) const;
	/** Check whether the polygon is skew. Returns true if there doesn't exist a
	 * plane in which the polygon can fit. \sa getBestFittingPlane */
	bool isSkew() const;
	/** Remove polygon's repeated vertices. */
	void removeRepeatedVertices();
	/** Erase every redundant vertex, thus saving space. */
	void removeRedundantVertices();
	/** Default constructor. Creates a polygon with no vertices.  */
	TPolygon3D() : std::vector<TPoint3D>() {}
	/** Constructor for a given size. Creates a polygon with a fixed number of
	 * vertices, which are initialized to garbage. */
	explicit TPolygon3D(size_t N) : std::vector<TPoint3D>(N) {}
	/** Implicit constructor from a 3D points vector. */
	TPolygon3D(const std::vector<TPoint3D>& v) : std::vector<TPoint3D>(v) {}
	/** Constructor from a 2D object. Zeroes the z. */
	explicit TPolygon3D(const TPolygon2D& p);
	/** Static method to create a regular polygon, given its size and radius.
	 * \throw std::logic_error if number of edges is less than three, or radius
	 * is near zero. */
	static void createRegularPolygon(
		size_t numEdges, double radius, TPolygon3D& poly);
	/** Static method to create a regular polygon, given its size and radius.
	 * The center will be located on the given pose.
	 * \throw std::logic_error if number of edges is less than three, or radius
	 * is near zero.
	 */
	static void createRegularPolygon(
		size_t numEdges, double radius, TPolygon3D& poly,
		const mrpt::math::TPose3D& pose);
};

/**
 * Object type identifier for TPoint2D or TPoint3D.
 * \sa TObject2D,TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_POINT = 0;
/**
 * Object type identifier for TSegment2D or TSegment3D.
 * \sa TObject2D,TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT = 1;
/**
 * Object type identifier for TLine2D or TLine3D.
 * \sa TObject2D,TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_LINE = 2;
/**
 * Object type identifier for TPolygon2D or TPolygon3D.
 * \sa TObject2D,TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_POLYGON = 3;
/**
 * Object type identifier for TPlane.
 * \sa TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_PLANE = 4;
/**
 * Object type identifier for empty TObject2D or TObject3D.
 * \sa TObject2D,TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_UNDEFINED = 255;

/**
 * Standard type for storing any lightweight 2D type. Do not inherit from this
 * class.
 * \sa TPoint2D,TSegment2D,TLine2D,TPolygon2D
 */
struct TObject2D
{
   private:
	/**
	 * Object type identifier.
	 */
	unsigned char type{GEOMETRIC_TYPE_UNDEFINED};
	/**
	 * Union type storing pointers to every allowed type.
	 */
	struct tobject2d_data_t
	{
		TPoint2D point;
		TSegment2D segment;
		TLine2D line;
		TPolygon2D* polygon{nullptr};

		tobject2d_data_t() = default;
	} data;
	/**
	 * Destroys the object, releasing the pointer to the content (if any).
	 */
	void destroy()
	{
		if (type == GEOMETRIC_TYPE_POLYGON) delete data.polygon;
		type = GEOMETRIC_TYPE_UNDEFINED;
	}

   public:
	/**
	 * Implicit constructor from point.
	 */
	TObject2D(const TPoint2D& p) : type(GEOMETRIC_TYPE_POINT)
	{
		data.point = p;
	}
	/**
	 * Implicit constructor from segment.
	 */
	TObject2D(const TSegment2D& s) : type(GEOMETRIC_TYPE_SEGMENT)
	{
		data.segment = s;
	}
	/**
	 * Implicit constructor from line.
	 */
	TObject2D(const TLine2D& r) : type(GEOMETRIC_TYPE_LINE) { data.line = r; }
	/**
	 * Implicit constructor from polygon.
	 */
	TObject2D(const TPolygon2D& p) : type(GEOMETRIC_TYPE_POLYGON)
	{
		data.polygon = new TPolygon2D(p);
	}
	/**
	 * Implicit constructor from polygon.
	 */
	TObject2D() : type(GEOMETRIC_TYPE_UNDEFINED) {}
	/**
	 * Object destruction.
	 */
	~TObject2D() { destroy(); }
	/**
	 * Checks whether content is a point.
	 */
	bool isPoint() const { return type == GEOMETRIC_TYPE_POINT; }
	/**
	 * Checks whether content is a segment.
	 */
	bool isSegment() const { return type == GEOMETRIC_TYPE_SEGMENT; }
	/**
	 * Checks whether content is a line.
	 */
	bool isLine() const { return type == GEOMETRIC_TYPE_LINE; }
	/**
	 * Checks whether content is a polygon.
	 */
	bool isPolygon() const { return type == GEOMETRIC_TYPE_POLYGON; }
	/**
	 * Gets content type.
	 */
	unsigned char getType() const { return type; }
	/**
	 * Gets the content as a point, returning false if the type is inadequate.
	 */
	bool getPoint(TPoint2D& p) const
	{
		if (isPoint())
		{
			p = data.point;
			return true;
		}
		else
			return false;
	}
	/**
	 * Gets the content as a segment, returning false if the type is
	 * inadequate.
	 */
	bool getSegment(TSegment2D& s) const
	{
		if (isSegment())
		{
			s = data.segment;
			return true;
		}
		else
			return false;
	}
	/**
	 * Gets the content as a line, returning false if the type is inadequate.
	 */
	bool getLine(TLine2D& r) const
	{
		if (isLine())
		{
			r = data.line;
			return true;
		}
		else
			return false;
	}
	/**
	 * Gets the content as a polygon, returning false if the type is
	 * inadequate.
	 */
	bool getPolygon(TPolygon2D& p) const
	{
		if (isPolygon())
		{
			p = *(data.polygon);
			return true;
		}
		else
			return false;
	}
	/**
	 * Assign another TObject2D. Pointers are not shared.
	 */
	TObject2D& operator=(const TObject2D& obj)
	{
		if (this == &obj) return *this;
		destroy();
		switch (type = obj.type)
		{
			case GEOMETRIC_TYPE_POINT:
				data.point = obj.data.point;
				break;
			case GEOMETRIC_TYPE_SEGMENT:
				data.segment = obj.data.segment;
				break;
			case GEOMETRIC_TYPE_LINE:
				data.line = obj.data.line;
				break;
			case GEOMETRIC_TYPE_POLYGON:
				data.polygon = new TPolygon2D(*(obj.data.polygon));
				break;
		}
		return *this;
	}
	/**
	 * Assign a point to this object.
	 */
	void operator=(const TPoint2D& p)
	{
		destroy();
		type = GEOMETRIC_TYPE_POINT;
		data.point = p;
	}
	/**
	 * Assign a segment to this object.
	 */
	void operator=(const TSegment2D& s)
	{
		destroy();
		type = GEOMETRIC_TYPE_SEGMENT;
		data.segment = s;
	}
	/**
	 * Assign a line to this object.
	 */
	void operator=(const TLine2D& l)
	{
		destroy();
		type = GEOMETRIC_TYPE_LINE;
		data.line = l;
	}
	/**
	 * Assign a polygon to this object.
	 */
	void operator=(const TPolygon2D& p)
	{
		destroy();
		type = GEOMETRIC_TYPE_POLYGON;
		data.polygon = new TPolygon2D(p);
	}
	/**
	 * Project into 3D space.
	 */
	void generate3DObject(TObject3D& obj) const;
	/**
	 * Constructor from another TObject2D.
	 */
	TObject2D(const TObject2D& obj) : type(GEOMETRIC_TYPE_UNDEFINED)
	{
		operator=(obj);
	}
	/**
	 * Static method to retrieve all the points in a vector of TObject2D.
	 */
	static void getPoints(
		const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts);
	/**
	 * Static method to retrieve all the segments in a vector of TObject2D.
	 */
	static void getSegments(
		const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms);
	/**
	 * Static method to retrieve all the lines in a vector of TObject2D.
	 */
	static void getLines(
		const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins);
	/**
	 * Static method to retrieve all the polygons in a vector of TObject2D.
	 */
	static void getPolygons(
		const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys);
	/**
	 * Static method to retrieve all the points in a vector of TObject2D,
	 * returning the remainder objects in another parameter.
	 */
	static void getPoints(
		const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts,
		std::vector<TObject2D>& remainder);
	/**
	 * Static method to retrieve all the segments in a vector of TObject2D,
	 * returning the remainder objects in another parameter.
	 */
	static void getSegments(
		const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms,
		std::vector<TObject2D>& remainder);
	/**
	 * Static method to retrieve all the lines in a vector of TObject2D,
	 * returning the remainder objects in another parameter.
	 */
	static void getLines(
		const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins,
		std::vector<TObject2D>& remainder);
	/**
	 * Static method to retrieve all the polygons in a vector of TObject2D,
	 * returning the remainder objects in another parameter.
	 */
	static void getPolygons(
		const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys,
		std::vector<TObject2D>& remainder);
};
/**
 * Standard object for storing any 3D lightweight object. Do not inherit from
 * this class.
 * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
 */
struct TObject3D
{
   private:
	/**
	 * Object type identifier.
	 */
	unsigned char type{GEOMETRIC_TYPE_UNDEFINED};
	/**
	 * Union containing pointer to actual data.
	 */
	struct tobject3d_data_t
	{
		TPoint3D point;
		TSegment3D segment;
		TLine3D line;
		TPolygon3D* polygon{nullptr};
		TPlane plane;

		tobject3d_data_t() = default;
	} data;
	/**
	 * Destroys the object and releases the pointer, if any.
	 */
	void destroy()
	{
		if (type == GEOMETRIC_TYPE_POLYGON) delete data.polygon;
		type = GEOMETRIC_TYPE_UNDEFINED;
	}

   public:
	/**
	 * Constructor from point.
	 */
	TObject3D(const TPoint3D& p) : type(GEOMETRIC_TYPE_POINT)
	{
		data.point = p;
	}
	/**
	 * Constructor from segment.
	 */
	TObject3D(const TSegment3D& s) : type(GEOMETRIC_TYPE_SEGMENT)
	{
		data.segment = s;
	}
	/**
	 * Constructor from line.
	 */
	TObject3D(const TLine3D& r) : type(GEOMETRIC_TYPE_LINE) { data.line = r; }
	/**
	 * Constructor from polygon.
	 */
	TObject3D(const TPolygon3D& p) : type(GEOMETRIC_TYPE_POLYGON)
	{
		data.polygon = new TPolygon3D(p);
	}
	/**
	 * Constructor from plane.
	 */
	TObject3D(const TPlane& p) : type(GEOMETRIC_TYPE_PLANE) { data.plane = p; }
	/**
	 * Empty constructor.
	 */
	TObject3D() : type(GEOMETRIC_TYPE_UNDEFINED) {}
	/**
	 * Destructor.
	 */
	~TObject3D() { destroy(); }
	/**
	 * Checks whether content is a point.
	 */
	bool isPoint() const { return type == GEOMETRIC_TYPE_POINT; }
	/**
	 * Checks whether content is a segment.
	 */
	bool isSegment() const { return type == GEOMETRIC_TYPE_SEGMENT; }
	/**
	 * Checks whether content is a line.
	 */
	bool isLine() const { return type == GEOMETRIC_TYPE_LINE; }
	/**
	 * Checks whether content is a polygon.
	 */
	bool isPolygon() const { return type == GEOMETRIC_TYPE_POLYGON; }
	/**
	 * Checks whether content is a plane.
	 */
	bool isPlane() const { return type == GEOMETRIC_TYPE_PLANE; }
	/**
	 * Gets object type.
	 */
	unsigned char getType() const { return type; }
	/**
	 * Gets the content as a point, returning false if the type is not
	 * adequate.
	 */
	bool getPoint(TPoint3D& p) const
	{
		if (isPoint())
		{
			p = data.point;
			return true;
		}
		else
			return false;
	}
	/**
	 * Gets the content as a segment, returning false if the type is not
	 * adequate.
	 */
	bool getSegment(TSegment3D& s) const
	{
		if (isSegment())
		{
			s = data.segment;
			return true;
		}
		else
			return false;
	}
	/**
	 * Gets the content as a line, returning false if the type is not adequate.
	 */
	bool getLine(TLine3D& r) const
	{
		if (isLine())
		{
			r = data.line;
			return true;
		}
		else
			return false;
	}
	/**
	 * Gets the content as a polygon, returning false if the type is not
	 * adequate.
	 */
	bool getPolygon(TPolygon3D& p) const
	{
		if (isPolygon())
		{
			p = *(data.polygon);
			return true;
		}
		else
			return false;
	}
	/**
	 * Gets the content as a plane, returning false if the type is not
	 * adequate.
	 */
	bool getPlane(TPlane& p) const
	{
		if (isPlane())
		{
			p = data.plane;
			return true;
		}
		else
			return false;
	}
	/**
	 * Assigns another object, creating a new pointer if needed.
	 */
	TObject3D& operator=(const TObject3D& obj)
	{
		if (this == &obj) return *this;
		destroy();
		switch (type = obj.type)
		{
			case GEOMETRIC_TYPE_POINT:
				data.point = obj.data.point;
				break;
			case GEOMETRIC_TYPE_SEGMENT:
				data.segment = obj.data.segment;
				break;
			case GEOMETRIC_TYPE_LINE:
				data.line = obj.data.line;
				break;
			case GEOMETRIC_TYPE_POLYGON:
				data.polygon = new TPolygon3D(*(obj.data.polygon));
				break;
			case GEOMETRIC_TYPE_PLANE:
				data.plane = obj.data.plane;
				break;
			case GEOMETRIC_TYPE_UNDEFINED:
				break;
			default:
				THROW_EXCEPTION("Invalid TObject3D object");
		}
		return *this;
	}
	/**
	 * Assigns a point to this object.
	 */
	void operator=(const TPoint3D& p)
	{
		destroy();
		type = GEOMETRIC_TYPE_POINT;
		data.point = p;
	}
	/**
	 * Assigns a segment to this object.
	 */
	void operator=(const TSegment3D& s)
	{
		destroy();
		type = GEOMETRIC_TYPE_SEGMENT;
		data.segment = s;
	}
	/**
	 * Assigns a line to this object.
	 */
	void operator=(const TLine3D& l)
	{
		destroy();
		type = GEOMETRIC_TYPE_LINE;
		data.line = l;
	}
	/**
	 * Assigns a polygon to this object.
	 */
	void operator=(const TPolygon3D& p)
	{
		destroy();
		type = GEOMETRIC_TYPE_POLYGON;
		data.polygon = new TPolygon3D(p);
	}
	/**
	 * Assigns a plane to this object.
	 */
	void operator=(const TPlane& p)
	{
		destroy();
		type = GEOMETRIC_TYPE_PLANE;
		data.plane = p;
	}
	/**
	 * Projects into 2D space.
	 * \throw std::logic_error if the 3D object loses its properties when
	 * projecting into 2D space (for example, it's a plane or a vertical line).
	 */
	void generate2DObject(TObject2D& obj) const
	{
		switch (type)
		{
			case GEOMETRIC_TYPE_POINT:
				obj = TPoint2D(data.point);
				break;
			case GEOMETRIC_TYPE_SEGMENT:
				obj = TSegment2D(data.segment);
				break;
			case GEOMETRIC_TYPE_LINE:
				obj = TLine2D(data.line);
				break;
			case GEOMETRIC_TYPE_POLYGON:
				obj = TPolygon2D(*(data.polygon));
				break;
			case GEOMETRIC_TYPE_PLANE:
				throw std::logic_error("Too many dimensions");
			default:
				obj = TObject2D();
				break;
		}
	}
	/**
	 * Constructs from another object.
	 */
	TObject3D(const TObject3D& obj) : type(GEOMETRIC_TYPE_UNDEFINED)
	{
		operator=(obj);
	}
	/**
	 * Static method to retrieve every point included in a vector of objects.
	 */
	static void getPoints(
		const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts);
	/**
	 * Static method to retrieve every segment included in a vector of objects.
	 */
	static void getSegments(
		const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms);
	/**
	 * Static method to retrieve every line included in a vector of objects.
	 */
	static void getLines(
		const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins);
	/**
	 * Static method to retrieve every plane included in a vector of objects.
	 */
	static void getPlanes(
		const std::vector<TObject3D>& objs, std::vector<TPlane>& plns);
	/**
	 * Static method to retrieve every polygon included in a vector of objects.
	 */
	static void getPolygons(
		const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys);
	/**
	 * Static method to retrieve every point included in a vector of objects,
	 * returning the remaining objects in another argument.
	 */
	static void getPoints(
		const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts,
		std::vector<TObject3D>& remainder);
	/**
	 * Static method to retrieve every segment included in a vector of objects,
	 * returning the remaining objects in another argument.
	 */
	static void getSegments(
		const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms,
		std::vector<TObject3D>& remainder);
	/**
	 * Static method to retrieve every line included in a vector of objects,
	 * returning the remaining objects in another argument.
	 */
	static void getLines(
		const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins,
		std::vector<TObject3D>& remainder);
	/**
	 * Static method to retrieve every plane included in a vector of objects,
	 * returning the remaining objects in another argument.
	 */
	static void getPlanes(
		const std::vector<TObject3D>& objs, std::vector<TPlane>& plns,
		std::vector<TObject3D>& remainder);
	/**
	 * Static method to retrieve every polygon included in a vector of objects,
	 * returning the remaining objects in another argument.
	 */
	static void getPolygons(
		const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
		std::vector<TObject3D>& remainder);
};

/** 2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
 * \sa mrpt::math::TTwist3D, mrpt::math::TPose2D
 */
struct TTwist2D
{
	enum
	{
		static_size = 3
	};
	/** Velocity components: X,Y (m/s) */
	double vx{.0}, vy{.0};
	/** Angular velocity (rad/s) */
	double omega{.0};

	/** Constructor from components */
	constexpr TTwist2D(double vx_, double vy_, double omega_)
		: vx(vx_), vy(vy_), omega(omega_)
	{
	}
	/** Default fast constructor. Initializes to zeros  */
	TTwist2D() = default;
	/** Coordinate access using operator[]. Order: vx,vy,vphi */
	double& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return vx;
			case 1:
				return vy;
			case 2:
				return omega;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Coordinate access using operator[]. Order: vx,vy,vphi */
	constexpr const double& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return vx;
			case 1:
				return vy;
			case 2:
				return omega;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Transformation into vector */
	void getAsVector(std::vector<double>& v) const
	{
		v.resize(3);
		v[0] = vx;
		v[1] = vy;
		v[2] = omega;
	}
	/** Transform the (vx,vy) components for a counterclockwise rotation of
	 * `ang` radians. */
	void rotate(const double ang);
	bool operator==(const TTwist2D& o) const;
	bool operator!=(const TTwist2D& o) const;
	/** Returns the pose increment of multiplying each twist component times
	 * "dt" seconds. */
	mrpt::math::TPose2D operator*(const double dt) const;
	/** Returns a human-readable textual representation of the object (eg: "[vx
	 * vy omega]", omega in deg/s)
	 * \sa fromString
	 */
	void asString(std::string& s) const;
	std::string asString() const
	{
		std::string s;
		asString(s);
		return s;
	}

	/** Set the current object value from a string generated by 'asString' (eg:
	 * "[0.02 1.04 -45.0]" )
	 * \sa asString
	 * \exception std::exception On invalid format
	 */
	void fromString(const std::string& s);
	static size_t size() { return 3; }
};

/** 3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
 * \sa mrpt::math::TTwist2D, mrpt::math::TPose3D
 */
struct TTwist3D
{
	enum
	{
		static_size = 6
	};
	/** Velocity components: X,Y (m/s) */
	double vx{.0}, vy{.0}, vz{.0};
	/** Angular velocity (rad/s) */
	double wx{.0}, wy{.0}, wz{.0};

	/** Constructor from components */
	constexpr TTwist3D(
		double vx_, double vy_, double vz_, double wx_, double wy_, double wz_)
		: vx(vx_), vy(vy_), vz(vz_), wx(wx_), wy(wy_), wz(wz_)
	{
	}
	/** Default fast constructor. Initializes to zeros  */
	TTwist3D() = default;
	/** Coordinate access using operator[]. Order: vx,vy,vphi */
	double& operator[](size_t i)
	{
		switch (i)
		{
			case 0:
				return vx;
			case 1:
				return vy;
			case 2:
				return vz;
			case 3:
				return wx;
			case 4:
				return wy;
			case 5:
				return wz;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Coordinate access using operator[]. Order: vx,vy,vphi */
	constexpr const double& operator[](size_t i) const
	{
		switch (i)
		{
			case 0:
				return vx;
			case 1:
				return vy;
			case 2:
				return vz;
			case 3:
				return wx;
			case 4:
				return wy;
			case 5:
				return wz;
			default:
				throw std::out_of_range("index out of range");
		}
	}
	/** Transformation into vector */
	void getAsVector(std::vector<double>& v) const
	{
		v.resize(6);
		for (int i = 0; i < 6; i++) v[i] = (*this)[i];
	}
	bool operator==(const TTwist3D& o) const;
	bool operator!=(const TTwist3D& o) const;
	/** Returns a human-readable textual representation of the object (eg: "[vx
	 * vy vz wx wy wz]", omegas in deg/s)
	 * \sa fromString
	 */
	void asString(std::string& s) const;
	std::string asString() const
	{
		std::string s;
		asString(s);
		return s;
	}

	/** Transform all 6 components for a change of reference frame from "A" to
	 * another frame "B" whose rotation with respect to "A" is given by `rot`.
	 * The translational part of the pose is ignored */
	void rotate(const mrpt::math::TPose3D& rot);

	/** Set the current object value from a string generated by 'asString' (eg:
	 * "[vx vy vz wx wy wz]" )
	 * \sa asString
	 * \exception std::exception On invalid format
	 */
	void fromString(const std::string& s);
	static size_t size() { return 3; }
};

// Binary streaming functions
template <
	class PoseOrPoint, typename = std::enable_if_t<std::is_base_of_v<
						   mrpt::math::TPoseOrPoint, PoseOrPoint>>>
mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, PoseOrPoint& o)
{
	for (int i = 0; i < o.static_size; i++) in >> o[i];
	return in;
}
template <
	class PoseOrPoint, typename = std::enable_if_t<std::is_base_of_v<
						   mrpt::math::TPoseOrPoint, PoseOrPoint>>>
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const PoseOrPoint& o)
{
	for (int i = 0; i < o.static_size; i++) out << o[i];
	return out;
}

mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, mrpt::math::TSegment2D& s);
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const mrpt::math::TSegment2D& s);

mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, mrpt::math::TLine2D& l);
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const mrpt::math::TLine2D& l);

mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, mrpt::math::TObject2D& o);
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const mrpt::math::TObject2D& o);

mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, mrpt::math::TSegment3D& s);
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const mrpt::math::TSegment3D& s);

mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, mrpt::math::TLine3D& l);
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const mrpt::math::TLine3D& l);

mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, mrpt::math::TPlane& p);
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const mrpt::math::TPlane& p);

mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, mrpt::math::TObject3D& o);
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const mrpt::math::TObject3D& o);

mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, mrpt::math::TTwist2D& o);
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const mrpt::math::TTwist2D& o);

mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, mrpt::math::TTwist3D& o);
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out, const mrpt::math::TTwist3D& o);

/** @} */  // end of grouping

}  // end of namespace math

namespace typemeta
{
// Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPoint2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPoint3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPose2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPose3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TSegment2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TLine2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPolygon2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TObject2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TSegment3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TLine3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPlane, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPolygon3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TObject3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TTwist2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TTwist3D, mrpt::math)

}  // namespace typemeta
}  // namespace mrpt
